function [lam_best, x_best, varargout] = lcurve(A, b, varargin)
%lcurve 绘制超定方程 Ax=b 的L-curve曲线, 返回最优的正则项系数
% 推荐阅读:
%   The Use of the L-Curve in the Regularization of Discrete Ill-Posed Problems
%
% Syntax:
%   [lam_best, x_best, rho_best, eta_best] = lcurve(A, b [ ...
%          'Show', false, 'LSampleNum', 200 ]);                        
%
%   [lam_best, x_best, rho_best, eta_best] = lcurve(___);
%
% Params:
%   - A          [required]  [numeric;2d] 方程左侧的系数矩阵 A
%   - b          [required]  [numeric;2d] 方程右侧的观测向量 b
%   - Show       [namevalue] [logical;scalar] 是否可视化Ax=b的L曲线
%   - LSampleNum [namevalue] [numeric;scalar] L曲线的采样点数
%
% Return:
%   - lam_best L曲线拐点处的最优正则化系数
%   - x_best 最优正则化系数下估计的参数向量
%   - rho_best 最优正则化系数下的残差向量模值平方 |Ax-b|^2
%   - eta_best 最优正则化系数下的正则项的模值平方 |x|^2
%
% Author:
%   iam002, 2024年10月31日
%
% Update:
%   iam002, 2024年10月31日
%
% Matlab Version: R2023a

    %% 解析Name-value
    % 输入解析器
    input_parser = inputParser;

    % 是否绘制L-曲线, 默认为 false
    addParameter(input_parser, 'Show', false, @islogical);

    % L-曲线的采样点数, 默认为 100
    addParameter(input_parser, 'LSampleNum', 200, @isnumeric);

    % 解析参数
    parse(input_parser, varargin{:})
    is_show = input_parser.Results.Show;
    num_points = input_parser.Results.LSampleNum;

    %% 参数检查
    if size(A, 1) < size(A, 2)
        error("The number of rows in matrix A must be no less than the number of columns in the matrix!\n");
    end

    %%
    % 执行 SVD 分解
    % A:(m,n), U:(m,n), V:(n,n), sig:(n,1)
    [U, sig, V] = svd(A, "econ");
    sig = diag(sig);
    sig2 = sig.^2;

    % 式(15)
    beta = U'*b; % (n,1)

    % 式(7) 求最小二乘解的残差模平方 |Ax-b|^2
    b0 = b - sum(U.*beta', 2); % (n,1)
    rho2_LS = b0'*b0;

    % 式(8)
    xi = safeDivide(beta, sig);

    % 正则化系数范围
    min_ratio = 16 * eps;
    lam_vec = zeros(num_points, 1);
    lam_vec(end) =  max(sig(end), sig(1)*min_ratio);
    ratio = (sig(1) / sig(end))^(1 / (num_points - 1));

    if sig(1) == sig(end) || ratio == 1
        warning("sig(1) == sig(end)!")
        return;
    end

    for i = (num_points - 1):-1:1
        lam_vec(i) = ratio * lam_vec(i + 1);
    end

    % 寻找拐点
    [lam_best, k_best, rho_best, eta_best] = l_corner(lam_vec, sig2, beta, xi, rho2_LS);

    % 求解正则化最小二乘解
    f_mat = sig2' ./ (sig2' + lam_best.^2); % (1,n)
    x_best = sum((xi'.*f_mat).*V, 2);

    if nargout >= 3
        varargout{1} = rho_best;
    end

    if nargout >= 4
        varargout{2} = eta_best;
    end

    if is_show
        % 计算曲率
        k = l_curvature(lam_vec, sig2, beta, xi, rho2_LS);
        % f_mat: (P, n)
        f_mat = sig2' ./ (sig2.' + lam_vec.^2);
        % eta: (P,1)
        eta = sum( (xi'.*f_mat).^2, 2);
        % rho: (P,1)
        rho = sum( (beta'.*(1 - f_mat)).^2, 2 ) + rho2_LS;

        figure;
        loglog(sqrt(rho), sqrt(eta), 'LineStyle', '-', 'Marker', '.', 'LineWidth', 1);
        hold on;
        plot(sqrt(rho_best), sqrt(eta_best), 'LineStyle', 'none', 'Marker', 'x', 'Color', 'r');
        xlabel('Residual norm ||Ax - b||');
        ylabel('Solution norm ||x||');
        grid on;
        hold on;
        if numel(lam_vec) >= 20
            N = 6;
            tmp = floor(num_points / N);
            id = tmp*(1:N); 
            tmp_lam = lam_vec(id);
            tmp_x = sqrt(rho(id));
            tmp_y = sqrt(eta(id));
            for i = 1:numel(tmp_x)
                text(tmp_x(i), tmp_y(i), sprintf('\\lambda = %f', tmp_lam(i)));
            end
        end

        figure;
        plot(lam_vec, -1*k, 'LineStyle', '-', 'Marker', '.', 'LineWidth', 1);
        hold on;
        plot(lam_best, k_best, 'Marker', 'x', 'Color', 'r');
        ylabel('Curvature');
        xlabel('Regulation \lambda');
        grid on;
    end
end

function res = safeDivide(num, den)
% 除法但是规定分母为0时输出也为0
    res = zeros(size(num));
    mask = den ~= 0;
    res(mask) = num(mask) ./ den(mask);
end

function [lam_best, k_best, rho_best, eta_best] = l_corner(lam_vec, sig2, beta, xi, rho2_LS)
% 寻找曲率最大的拐点(lam_vec的个数不能太小)

    % 获取lam_vec对应的曲率
    k = l_curvature(lam_vec, sig2, beta, xi, rho2_LS);

    [~, min_kid] = min(k);
    lam_left = lam_vec(min(min_kid + 1, numel(k)));
    lam_right = lam_vec(max(1, min_kid - 1));

    fun_loss = @(lam)(l_curvature(lam, sig2, beta, xi, rho2_LS));

    [lam_best, k_best, is_found] = fminbnd(fun_loss, lam_left, lam_right);
    k_best = -1*k_best;
    
    if ~is_found
        warning("hs.math.opt.fminbnd failed!")
        lam_best = lam_vec(1);
    end
    
    if k_best < 0
        warning("The maximum curvature is less than 0!");
        lam_best = lam_vec(1);
    end

    f_mat = sig2' ./ (sig2.' + lam_best.^2); % f_mat: (P, n)
    eta_best = sum( (xi'.*f_mat).^2, 2); % eta: (P,1)
    rho_best = sum( (beta'.*(1 - f_mat)).^2, 2) + rho2_LS; % rho: (P,1)

end

function k = l_curvature(lam_vec, sig2, beta, xi, rho2_LS)
% L-Curve的曲率计算函数 

    % 维度说明:
    % lam_vec: (P,1)
    % sig2: (n,1)
    % beta: (n,1)
    % xi:   (n,1)
    % rho2_LS: (1,1) 
    %
    % k: (P,1)

    f_mat = sig2' ./ (sig2.' + lam_vec.^2); % (P, n)
    eta = sum( (xi'.*f_mat).^2, 2); % (P,1)
    rho = sum( (beta'.*(1 - f_mat)).^2, 2) + rho2_LS; % (P,1)
    d_eta = -4./lam_vec.*sum( (xi'.*f_mat).^2.*(1 - f_mat), 2); % (P,1)
    
    % 式(18)
    num1 = 2.*eta.*rho;
    num2 = (lam_vec.^2).*d_eta.*rho;
    num3 = 2.*lam_vec.*eta.*rho;
    num4 = (lam_vec.^4).*eta.*d_eta;
    num = num1.*(num2 + num3 + num4);
    den = d_eta.*((lam_vec.^4).*eta.^2 + rho.^2).^(3/2);
    k = num./den; % 这里的负号取消掉, 以配合 fminbnd 找最小值
end

    